#include "xyz_lla.h"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <cmath>
#include <iostream>

Xyz_trans::Xyz_trans(const sensor_msgs::NavSatFix &gps_fix,
                     const geometry_msgs::TwistWithCovarianceStamped &gps_vel,
                     const nav_msgs::Odometry &gps_odom,
                     const pcl::PointXYZ &xyz) {
  log_lat_alt_.log = gps_fix.longitude;
  log_lat_alt_.lat = gps_fix.latitude;
  log_lat_alt_.alt = gps_fix.altitude;
  origin_xyz_.x = xyz.x;
  origin_xyz_.y = xyz.y;
  origin_xyz_.z = xyz.z;
  qua_heading_.x() = gps_odom.pose.pose.orientation.x;
  qua_heading_.y() = gps_odom.pose.pose.orientation.y;
  qua_heading_.z() = gps_odom.pose.pose.orientation.z;
  qua_heading_.w() = gps_odom.pose.pose.orientation.w;
}

void Xyz_trans::CurrentPosShow() {
  std::cout << "当前经纬高(" << log_lat_alt_.log << "," << log_lat_alt_.lat
            << "," << log_lat_alt_.alt << "),";
  std::cout << "待转换点坐标(" << origin_xyz_.x << "," << origin_xyz_.y << ","
            << origin_xyz_.z << ")" << std::endl;
}

Angle Xyz_trans::CalculateAngle() {

  Angle tmp{0, 0};
  // 直接求解角度
  car_angle_.sin=2*qua_heading_.w()*qua_heading_.z();
  car_angle_.cos=1-2*qua_heading_.z()*qua_heading_.z();
	double angle=90/pi_*acos(qua_heading_.w());
  std::cout<<"四元数直接输出的角度："<<angle<<std::endl;
  //

  // 上面是根据速度解算的角度,以下是直接读gnss四元数然后转换到角度
  Eigen::Vector3d euler = qua_heading_.toRotationMatrix().eulerAngles(2, 1, 0);
  double north_angle = euler[0] * 180 / pi_;
//  std::cout<<",解算的角度："<<north_angle<<"角度差值："<<north_angle-angle<<std::endl;
//  car_angle_.sin = std::sin(north_angle);
//  car_angle_.cos = std::cos(north_angle);
  tmp.sin = car_angle_.sin;
  tmp.cos = car_angle_.cos;

  return tmp;
}

LogLatAlt Xyz_trans::CalculateLla() {
  // 先把坐标点从gnss坐标系转换到正北坐标系的点
  pcl::PointXYZ transfered_xyz{0.0, 0, 0};
  transfered_xyz.x =
      origin_xyz_.x * car_angle_.cos - origin_xyz_.y * car_angle_.sin;
  transfered_xyz.y =
      origin_xyz_.x * car_angle_.sin + origin_xyz_.y * car_angle_.cos;
  transfered_xyz.z = origin_xyz_.z;

  LogLatAlt tmp{0, 0, 0};
  tmp.log = log_lat_alt_.log - (transfered_xyz.y * 180) /
                                   ( pi_* r_earth * std::cos(log_lat_alt_.lat));
  tmp.lat = log_lat_alt_.lat + (transfered_xyz.x * 180) / ( pi_* r_earth);
  tmp.alt = log_lat_alt_.alt + transfered_xyz.z;

  return tmp;
}
